package com.hongji.lms5xxcommunicator.scanner.config;

import java.util.Locale;

/**
 * Livox 配置（兼容你原有字段，并新增安装姿态/侧别/PLC-X 融合/XYZ 反转与缩放）。
 * 说明：
 * - yaw/pitch/roll 使用 Z(=yaw) → Y(=pitch) → X(=roll) 顺序（度）。
 * - side 影响与 PLC-X 融合公式：X = plcX ± localX + xOffset（RIGHT:+，LEFT:-，CENTER 不融合）。
 * - coord.* 中的 y_base 与 z_offset 依旧保留，策略内会按你的逻辑叠加到 yOffset / zOffset。
 */
public class LivoxConfig {

    // —— 端口与解包 —— //
    public int udpPort = 56301;
    public int recvBufBytes = 4 * 1024 * 1024;

    // 头部/偏移
    public int offDotNum = 5;      // u16
    public int offDataType = 10;   // u8
    public int headerBytes = 18;   // 负载起始偏移

    // 每点步长/尾随跳过
    public int strideType1 = 12;   // data_type=1：int32*3（mm）
    public int strideType2 = 6;    // data_type=2：int16*3（×10mm）
    public int skipAfter1 = 0;
    public int skipAfter2 = 0;

    // 调试
    public boolean debugHexdump = false;

    // —— 窗口过滤（世界系） —— //
    // ✅ 新增 X 过滤（默认给一个极大窗口）
    public double xMin = -9.9E9;
    public double xMax =  9.9E9;

    public double zMin = -500;
    public double zMax = 9000;
    public double yMin = -10000;
    public double yMax = 10000;

    // —— 坐标统一（与 LMS 同语义，保留原字段） —— //
    public boolean coordEnabled = true;
    public double yBase = 0;           // 会叠加到 yOffset
    public double zOffset = 0;         // 会叠加到 zOffset（与 h0 叠加）
    public double yScale = 1.0;
    public double zScale = 1.0;
    public boolean invertY = false;
    public boolean invertZ = false;

    // ✅ 新增：X 轴缩放/反转（原版只管 Y/Z，这里补齐）
    public double xScale = 1.0;
    public boolean invertX = false;

    // —— Livox 外参/朝向（侧装/竖装） —— //
    public double h0 = 0;          // LiDAR 原点离地标高（mm），作用到 Z
    public double xOffset = 0;     // 世界 X 平移
    public double yOffset = 0;     // 世界 Y 平移

    // 旧版符号位（保留）：历史兼容
    public int sX = 1;             // 仅作方向因子使用
    public int sY = 1;
    public int sZ = 1;

    // ✅ 新增：安装姿态（欧拉角，度）
    public double yawDeg = 0.0;    // 绕 Z
    public double pitchDeg = 0.0;  // 绕 Y
    public double rollDeg = 0.0;   // 绕 X

    // ✅ 新增：左右侧放置，用于与 PLC-X 融合（RIGHT/LEFT/CENTER）
    public enum MountSide { LEFT, RIGHT, CENTER }
    public MountSide side = MountSide.RIGHT;

    // ✅ 新增：是否把局部 X 与 PLC-X 融合（联调期可先关）
    public boolean plcMergeEnabled = false;

    // —— 最近邻 PLC-X 容差（ms） —— //
    public int xLookupTolMs = 30;

    // —— 是否由本实例托管 XSource 生命周期（多机组合里可关） —— //
    public boolean manageXSourceLifecycle = true;

    // ✅ 新增：预计算旋转矩阵（Rz*Ry*Rx），行主序
    public double[] R = new double[9];

    /** 计算安装姿态旋转矩阵（度→弧度；Rz*Ry*Rx） */
    public LivoxConfig recomputeRotation() {
        final double rz = Math.toRadians(yawDeg);
        final double ry = Math.toRadians(pitchDeg);
        final double rx = Math.toRadians(rollDeg);

        final double cz = Math.cos(rz), sz = Math.sin(rz);
        final double cy = Math.cos(ry), sy = Math.sin(ry);
        final double cx = Math.cos(rx), sx = Math.sin(rx);

        // Rz * Ry * Rx
        final double r00 = cz * cy;
        final double r01 = cz * sy * sx - sz * cx;
        final double r02 = cz * sy * cx + sz * sx;

        final double r10 = sz * cy;
        final double r11 = sz * sy * sx + cz * cx;
        final double r12 = sz * sy * cx - cz * sx;

        final double r20 = -sy;
        final double r21 = cy * sx;
        final double r22 = cy * cx;

        R[0]=r00; R[1]=r01; R[2]=r02;
        R[3]=r10; R[4]=r11; R[5]=r12;
        R[6]=r20; R[7]=r21; R[8]=r22;
        return this;
    }

    @Override
    public String toString() {
        return String.format(Locale.ROOT,
                "{udp=%d, recv=%d, hdr(offDotNum=%d,offDataType=%d,hb=%d), stride(t1=%d,t2=%d,sk1=%d,sk2=%d), " +
                        "filter(x[%.1f,%.1f], y[%.1f,%.1f], z[%.1f,%.1f]), coord(en=%s, xScale=%.2f, yScale=%.2f, zScale=%.2f, invX=%s, invY=%s, invZ=%s, yBase=%.1f, zOff=%.1f), " +
                        "ext(h0=%.1f, xOff=%.1f, yOff=%.1f, sX=%d,sY=%d,sZ=%d), mount(side=%s,yaw=%.1f,pitch=%.1f,roll=%.1f), plcMerge=%s, xTolMs=%d, manageXSrc=%s}",
                udpPort, recvBufBytes, offDotNum, offDataType, headerBytes,
                strideType1, strideType2, skipAfter1, skipAfter2,
                xMin, xMax, yMin, yMax, zMin, zMax,
                String.valueOf(coordEnabled), xScale, yScale, zScale,
                String.valueOf(invertX), String.valueOf(invertY), String.valueOf(invertZ), yBase, zOffset,
                h0, xOffset, yOffset, sX, sY, sZ,
                side, yawDeg, pitchDeg, rollDeg,
                String.valueOf(plcMergeEnabled), xLookupTolMs, String.valueOf(manageXSourceLifecycle));
    }
}
